/*
  three_Servos
  2022-8-22 

  本程序旨在使用Da-Servo库让4个舵机同时往复运动
  This example code shows how to use Da-Servo lib to
  control four servos move simultaneously.


*/

#include "DaServo.h"

#define startPos 0    //舵机往复运动起始位置 / Start position of servo move back and forth
#define endPos 135    //舵机往复运动结束位置 / End position of servo move back and forth
#define servoSpeed 500 //舵机运行速度 / Servo Speed

DaServo myservo1(0, servoSpeed); //建立舵机对象1
DaServo myservo2(1, servoSpeed); //建立舵机对象2
DaServo myservo3(2, servoSpeed); //建立舵机对象3
DaServo myservo4(3, servoSpeed); //建立舵机对象4

// 自定义IIC引脚
static const uint8_t mSDA = 8; // esp32默认为8
static const uint8_t mSCL = 6; // esp32默认为9

void setup()
{

  Wire.setPins(mSDA, mSCL);

  myservo1.init();
  myservo2.init();
  myservo3.init();
  myservo4.init();

  myservo1.setTarget(endPos); //初始设定舵机1运动目标位置 / myservo1 initial target
  myservo2.setTarget(endPos); //初始设定舵机2运动目标位置 / myservo2 initial target
  myservo3.setTarget(endPos); //初始设定舵机3运动目标位置 / myservo3 initial target
  myservo4.setTarget(endPos); //初始设定舵机4运动目标位置 / myservo4 initial target
}

void loop()
{
  //舵机1往复运动 / myservo1 moves back and forth
  if (myservo1.getPosition() == endPos)
  {
    myservo1.setTarget(startPos);
  }
  else if (myservo1.getPosition() == startPos)
  {
    myservo1.setTarget(endPos);
  }

  //舵机2往复运动 / myservo2 moves back and forth
  if (myservo2.getPosition() == endPos)
  {
    myservo2.setTarget(startPos);
  }
  else if (myservo2.getPosition() == startPos)
  {
    myservo2.setTarget(endPos);
  }

  //舵机3往复运动 / myservo3 moves back and forth
  if (myservo3.getPosition() == endPos)
  {
    myservo3.setTarget(startPos);
  }
  else if (myservo3.getPosition() == startPos)
  {
    myservo3.setTarget(endPos);
  }

  //舵机4往复运动 / myservo4 moves back and forth
  if (myservo4.getPosition() == endPos)
  {
    myservo4.setTarget(startPos);
  }
  else if (myservo4.getPosition() == startPos)
  {
    myservo4.setTarget(endPos);
  }

  //重要：servoUpdate函数用于更新舵运行状态，必须经常调用否则舵机无法正常运行!
  // IMPORTANT: servoUpdate MUST be called constantly or servo CANNOT work!
  myservo1.servoUpdate();
  myservo2.servoUpdate();
  myservo3.servoUpdate();
  myservo4.servoUpdate();
}
